const START = 2;
const END = 3;
const FLOOR = 0;
const BARRIER = 1;


function heuristic_cost_estimate(node, goal = {x:0, y: 3}){
  return distance(node,goal);
}

function distance(node, goal){
  return Math.sqrt(Math.pow(goal.x - node.x, 2) + Math.pow(goal.y - node.y, 2));
}


function nabors(node){
  return [
    {x: node.x -1, y: node.y},
    {x: node.x +1, y: node.y},
    {x: node.x, y: node.y + 1},
    {x: node.x, y: node.y - 1},
  ]
}

/*
const map = [
  [1, 99, 0, 2],
  [0, 99, 99, 0],
  [0, 0, 0, 0],
  [0, 99, 0, 0],
]
*/

function is_barrier(node, map){
  return map[node.y] == null || map[node.y][node.x] == null || map[node.y][node.x] == 1;
}

function node_equal(node1, node2){
  return node1.x == node2.x && node1.y == node2.y;
}

function in_set(set, node){
  return set.find(n => {
    return node_equal(n, node);
  });
}

function not_barrier_nabors(node, map){
  return nabors(node).filter(n => {return !is_barrier(n, map)})
}

function hash(node){
  return JSON.stringify(node);
}

function reconstruct_path(cameFrom, goal){
  let current = goal;
  let result = [];
  while(current != null){
    result.push(current);
    current = cameFrom[hash(current)];
  }
  return result;
}

//返回路径数组
function A_star(map){
  console.log(map);
  
  let start = {x: -99, y: -99};
  let goal = {x: -88, y: -88};
  if(map != null){
    for(let i=0;i<map.length;i++){
      for(let j=0;j<map[i].length;j++){
        if(map[i][j] == 2){
          start.x = j;
          start.y = i;
        }else if(map[i][j] == 3){
          goal.x = j;
          goal.y = i;
        }
      }
    }
  }else{
    throw "Bad map 1";
  }

  console.log(start, goal);

  //const nbs = not_barrier_nabors(start);
  let closeSet = [];
  let openSet = [start];
  const gScore = {
    [hash(start)]: 0,
  };
  const fScore = {
    [hash(start)]: heuristic_cost_estimate(start, goal),
  };

  const cameFrom = {};

  //Get minimum fScore in openSet

  let result = null;
  let counter = 0;
  while(true){
    counter ++;
    if(counter > 500){
      return "Logic error";
    }
    if(openSet.length < 1){
      console.error('Bad map 2, counter:', counter);
      result = [];
      break;
    }else{
      const current = (() => {
        let minimum = openSet[0];
        openSet.forEach(node => {
          if(fScore[hash(node)] < fScore[hash(minimum)]){
            minimum = node;
          }
        });
        return minimum;
      })();
  
      if(node_equal(current, goal)){
        result = reconstruct_path(cameFrom, goal);
        break;
      }
  
      openSet = openSet.filter(node => {
        //return hash(node) != hash(current)
        return !node_equal(node, current);
      });
      closeSet.push(current);
  
      const nbs = not_barrier_nabors(current, map)
        .filter(nb => { //Filter not in closeSet
          return closeSet.find(node => {
            return node_equal(node, nb);
          }) == null;
        })
        .forEach(nb => { //更新Openset, 可以走的点
          const tentative_gScore = gScore[hash(current)] + distance(nb, current);
  
          if(in_set(openSet, nb)){
            if(gScore[hash(nb)] > tentative_gScore){
              gScore[hash(nb)] = tentative_gScore;
              fScore[hash(nb)] = tentative_gScore + heuristic_cost_estimate(nb, goal);
              cameFrom[hash(nb)] = current;
            }
          }else{
            openSet.push(nb);
            gScore[hash(nb)] = tentative_gScore;
            fScore[hash(nb)] = tentative_gScore + heuristic_cost_estimate(nb);
            cameFrom[hash(nb)] = current;
          }
        })
    }
  }
  return result;
}

